#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/PointStamped.h>
#include<geometry_msgs/TransformStamped.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include<sensor_msgs/Imu.h>
#include <uav/convert_accel.h>
#include<visualization_msgs/Marker.h>
#include<nav_msgs/Path.h>
#include<std_msgs/Float64.h>

//上升结束
int flag_pos=0;
//计数
int count_all=0;
int count_out=0;
//全局变量
geometry_msgs::Vector3Stamped input_,output_;
geometry_msgs::PoseStamped pose_out;
geometry_msgs::TwistStamped last_twist;
geometry_msgs::Vector3Stamped acc_cal;
double t;

//convert_.h函数

geometry_msgs::Vector3Stamped optimization(geometry_msgs::Vector3Stamped input,geometry_msgs::PoseStamped pose_,geometry_msgs::TwistStamped twst);
int testPos(geometry_msgs::PoseStamped pos);

//回调函数
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose;
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg1)
{
    current_pose=*msg1; 
}

geometry_msgs::TwistStamped current_twist;
void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &msg2)
{
    current_twist= *msg2;
}

sensor_msgs::Imu current_imu;
void imu_cb(const sensor_msgs::Imu::ConstPtr &msg3)
{
    current_imu= *msg3;
}


int main(int argc, char** argv)
{
    ros::init(argc,argv,"ecbf_node");
    ros::NodeHandle nh;

    tf::TransformListener listener;


    //收发器
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber  current_vel=nh.subscribe<geometry_msgs::TwistStamped>
            ("/mavros/local_position/velocity_local",10,twist_cb);
    ros::Subscriber local_pos=nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose",10,&pose_cb);
    ros::Subscriber imu_info=nh.subscribe<sensor_msgs::Imu>
            ("/mavros/imu/data",10,imu_cb);

    ros::Publisher marker_pub=nh.advertise<visualization_msgs::Marker>("marker",10);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("trajectory", 10);
    ros::Publisher point_pub = nh.advertise<geometry_msgs::PointStamped>("point", 10);
    ros::Publisher pos_dst=nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local",10); 
    ros::Publisher accel_pub=nh.advertise<geometry_msgs::Vector3Stamped>
            ("/mavros/setpoint_accel/accel",10); 

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    input_.vector.x=4;
    input_.vector.y=4;
    input_.vector.z=2;

    ros::Rate rate(25.0);

    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }
    
    //位置控制
    pose_out.pose.position.x=0;
    pose_out.pose.position.y=0;
    pose_out.pose.position.z=1.5;


    for(int i=100;ros::ok()&&i>0;--i)
    {
        pos_dst.publish(pose_out);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    while(ros::ok())
    {

        if(current_state.mode!="OFFBOARD"&&
        (ros::Time::now()-last_request>ros::Duration(5.0)))
        {
            if(set_mode_client.call(offb_set_mode)&&
            offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request=ros::Time::now();
        }
        else
        {
            if(!current_state.armed&&
            (ros::Time::now()-last_request>ros::Duration(5.0)))
            {
                if(arming_client.call(arm_cmd)&&
                arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request=ros::Time::now();
            }
        }

        //spinOnce放在两次操作中
        if(flag_pos==0)
        {
            pos_dst.publish(pose_out);
            ros::spinOnce();
            rate.sleep();
            if(current_pose.pose.position.z>1.5)
            {
                last_twist=current_twist;
                t=ros::Time::now().toSec();
                flag_pos=1;
            }
        } 
        if(flag_pos==1)
        { 
            ROS_INFO("accel on");
            //pose发布
            ros::spinOnce();
            rate.sleep();

/*             geometry_msgs::Vector3Stamped imu_in,imu_out;
            imu_in.header.stamp=ros::Time(0);
            imu_in.header.frame_id="base_link";
            imu_in.vector.x=current_imu.linear_acceleration.x;
            imu_in.vector.y=current_imu.linear_acceleration.y;
            imu_in.vector.z=current_imu.linear_acceleration.z; */

            
            //ROS_INFO("IMU Input=%f, %f, %f",current_imu.linear_acceleration.x,current_imu.linear_acceleration.y,
            //current_imu.linear_acceleration.z-9.8);
            //ROS_INFO("IMU Input=%f, %f, %f",imu_out.vector.x,imu_out.vector.y,imu_out.vector.z);

            accel_pub.publish(input_);
        } 
    }
    return 0;


}